#include "matrix4f.hpp"

namespace sandbox
{
	namespace sdk
	{
		namespace vector_constants
		{
			const ALIGN_BEG(16) __m128i sign_mask ALIGN_END(16)		=QUAD(0x80000000);
			const ALIGN_BEG(16) __m128i ones ALIGN_END(16)			=QUAD(1);
			const ALIGN_BEG(16) __m128 onesf ALIGN_END(16)			=QUAD(1.0f);
			const ALIGN_BEG(16) __m128 halff ALIGN_END(16)			=QUAD(0.5f);
			const ALIGN_BEG(16) __m128i sign_mask_xyz ALIGN_END(16)  =
			{0x0,0x80000000,0x80000000,0x80000000};
		}
	}
}
static const ALIGN_BEG(16) __m128  coeff_4_div_pi ALIGN_END(16)	=QUAD( 1.27323954473516f);
static const ALIGN_BEG(16) __m128 DP1 ALIGN_END(16) 			=QUAD(-0.78515625f);
static const ALIGN_BEG(16) __m128 DP2 ALIGN_END(16) 			=QUAD(-2.4187564849853515625e-4f);
static const ALIGN_BEG(16) __m128 DP3 ALIGN_END(16) 			=QUAD(-3.77489497744594108e-8f);
static const ALIGN_BEG(16) __m128 SF0 ALIGN_END(16) 			=QUAD(-1.9515295891E-4f);
static const ALIGN_BEG(16) __m128 SF1 ALIGN_END(16) 			=QUAD(8.3321608736E-3f);
static const ALIGN_BEG(16) __m128 SF2 ALIGN_END(16) 			=QUAD(-1.6666654611E-1f);
static const ALIGN_BEG(16) __m128 CF0 ALIGN_END(16) 			=QUAD(2.443315711809948E-005f);
static const ALIGN_BEG(16) __m128 CF1 ALIGN_END(16) 			=QUAD(-1.388731625493765E-003f);
static const ALIGN_BEG(16) __m128 CF2 ALIGN_END(16) 			=QUAD(4.166664568298827E-002);

using namespace ::sandbox::sdk::vector_constants;

void sandbox::sdk::detail::sincos_ps(__m128 x, __m128& cos, __m128& sin)
{
    //VEC_NAND3(x, x, is);
	__m128 X    = _mm_nand_ps(sign_mask, x); 
	/* scale by 4/Pi */
    //  vec_int4   j;
    //  x1 = fpi * x;
	__m128 Y    = _mm_mul_ps(abs, coeff_4_div_pi);
	/* store the integer part of y in emm2 */
    //  VEC_F2I(j, x1); /* integer part of x/(PI/4) */
    //  j += j & i1;
	__m128i tmp = _mm_cvttps_epi32(Y);
	tmp         = _mm_add_epi32(tmp, _mm__si128(ones, tmp));
    //  VEC_I2F(y, j );
	Y           = _mm_cvtepi32_ps(tmp);
    //#define vec_nmsub(a,b,c)      ((c)-(a)*(b))
	//x = vec_nmsub(y, dp1,x); //<=x-y*dp1
    //x = vec_nmsub(y, dp2,x); 
    //x = vec_nmsub(y, dp3,x);
	X           = _mm_sub_ps(X, _mm_mul_ps(Y, DP1));
	X           = _mm_sub_ps(X, _mm_mul_ps(Y, DP2));
	X           = _mm_sub_ps(X, _mm_mul_ps(Y, DP3));
    //vec_float4 z = x * x;
	__m128 Z    = _mm_mul_ps(X,X);

	//vec_int4 sign_s = vec_and((vec_int4)x, is);
	// j &= i7;<=may be omited
	// vec_int4 sign_c  = -(vec_int4)VEC_SR(j & CI4_4,2) & is;	
    //sign_s  ^=  sign_c; // revert sign if j=4,6
	//====this is raw code, can be optimized by reducing and's====
	//__m128i sigs= _mm_and_si128((__m128i)x),sign_mask); 	
	//__m128i sigc= _mm_and_si128(_mm_slli_epi32(tmp, 29),sign_mask);
    //sigs        = _mm_xor_si128(sigs,sigc);
    // j         &=  i3;   // j= 0,2<==can be delayed
	// vec_int4 j_larger_1 = -VEC_SR(j & CI4_2,1);
    // sign_c ^=   j_larger_1 & is;
	//====this is raw code, can be optimized by reducing and's====
    //__m128i lgr1 = _mm_and_si128(,sign_mask);
    //sigc         = _mm_xor_si128(sigc, lgr1);
	//vec_int4 j_equals_1_or_2 = - VEC_SR( j & CI4_2, 1 );
	//====this is raw code, can be optimized by reducing and's====
	//__m128i je12= _mm_slli_epi32(tmp, 30);
    __m128i sigc= _mm_slli_epi32(tmp, 29);
	__m128i je12= _mm_slli_epi32(tmp, 30);
    __m128i sigs= _mm_xor_si128((__m128i)x, sigc);
    sigc        = _mm_xor_si128(sigc, je12);
	//now we can convert upper bits of je12 to full mask
	je12 		= _mm_srai_epi32(je12, 31);
	//y0  = (( cf0 * z + cf1 ) * z + cf2) * z * z - v05 * z + v1;
	__m128 y0,y1;
	y0 			= _mm_mul_ps(CF0,Z);
	y0 			= _mm_add_ps(y0, CF1);
	y0 			= _mm_mul_ps(y0, Z);
	y0 			= _mm_add_ps(y0, CF2);
	y0 			= _mm_mul_ps(y0, _mm_mul_ps(Z,Z));
	y0 			= _mm_sub_ps(y0, _mm_add_ps(_mm_mul_ps(Z, halff), onesf));
    //y1  = (( sf0 * z + sf1 ) * z + sf2) * z * x + x;
	y1 			= _mm_mul_ps(SF0,Z);
	y1 			= _mm_add_ps(y1, SF1);
	y1 			= _mm_mul_ps(y1, Z);
	y1 			= _mm_add_ps(y1, SF2);
	y1 			= _mm_mul_ps(y1, _mm_mul_ps(X,Z));
	y1 			= _mm_add_ps(y1, X);
    //  y = vec_sel(y1,y0,(vec_uint4)j_equals_1_or_2);
	Y  = (__m128)_mm_or_si128(
			_mm_nand(je12, (__m128i)y1),
			_mm_and (je12, (__m128i)y0));
	//VEC_XOR(y, sign_s);
    //*s = y;
	//we can apply sign mask here
	sin = (__m128)_mm_xor_si128((__m128i)Y, 
			_mm_and_si128(sign_mask, sigs));
    //y = vec_sel(y0,y1,(vec_uint4)j_equals_1_or_2);  
    //  VEC_XOR(y, sign_c);
    //  *c = y;       
	Y  = (__m128)_mm_or_si128(
			_mm_nand(je12, (__m128i)y0),
			_mm_and (je12, (__m128i)y1));
	cos = (__m128)_mm_xor_si128((__m128i)Y, 
			_mm_and_si128(sign_mask, sigc));
}
